#include "definitions.h"

#include "linkedqueue.h"
#include <stdlib.h>
#include <stdio.h>

// processor definitions
#include "inc/lm4f120h5qr.h"

#include "inc/hw_types.h"
#include "inc/hw_ints.h"
#include "inc/hw_memmap.h"
#include "driverlib/interrupt.h"
#include "driverlib/gpio.h"
#include "driverlib/rom.h"
#include "driverlib/rom_map.h"
#include "driverlib/sysctl.h"
#include "driverlib/timer.h"
#include "driverlib/adc.h"

#include "utils/uartstdio.h"
#include "driverlib/uart.h"

#include "inc/hw_sysctl.h"
#include "inc/hw_gpio.h"	//for unlocking

#include "uart.h"		//custom uart functions
#include "pwm.h"
#include "adc.h"
#include "timer0.h"
#include "timer1.h"
//#include "stepper.h"
#include "syn6288.h"

#include "step.h"

//PWM Define
//#define PWM PA_2


//int delay(int time);
unsigned long ulPeriod;
int count = 0;
int flip = 0;
int readValue = 0;
int qerror[2] = {0, 0};
int qstatus = 0;
int drive_go = 1;

//extern void Timer0IntHandler(void);
extern void PortAIntHandler(void);
extern void PortFIntHandler(void);
extern void UART0IntHandler(void);
extern void UART3IntHandler(void);
extern void UART4IntHandler(void);
void PortUnlock(void);
//void initPWM(int freq);
//extern void pwmHandler(void);
//void pwmSetDuty(int duty_cycle);
//void initADC0(void);


void belt_driver(int);

void initGPIO(void);
void initPeripheralClocks(void);

int main(void) {
	int firstRun=1;
	int temp=0;
	int deadzone=100;

	//enable clock, 200MHz/5 = 40MHz
	ROM_SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
	
	//enable clock, use crystal at 1MHz
	//ROM_SysCtlClockSet(SYSCTL_RCC_XTAL_1MHZ | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);

	initPeripheralClocks();
	//unlock PD7
	PortUnlock();

	initGPIO();

	initUART();

	//Stepper Shite
	initStepper();



	//GPIO interrupt setup
	//ROM_GPIOPinIntEnable(GPIO_PORTA_BASE, PA_2);		//added
	ROM_GPIOPinIntEnable(GPIO_PORTF_BASE, (btn2));

	//ROM_GPIOIntTypeSet(GPIO_PORTA_BASE, PA_2, GPIO_FALLING_EDGE);
	//ROM_GPIOPadConfigSet(GPIO_PORTA_BASE, PA_2, GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);

	ROM_GPIOIntTypeSet(GPIO_PORTF_BASE, (btn2), GPIO_FALLING_EDGE);
	ROM_GPIOPadConfigSet(GPIO_PORTF_BASE, (btn2), GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);
	
	//initial interrupt clear
	//ROM_GPIOPinIntClear(GPIO_PORTA_BASE, PA_2);
	ROM_GPIOPinIntClear(GPIO_PORTF_BASE, (btn2));

	ROM_IntEnable(INT_GPIOA);	//works!!!!!
	ROM_IntEnable(INT_GPIOF);


	//send test uart
	bt_UARTsend("\033[2J");				//clear screen
	bt_UARTsend("Initializing System....\r\n");
	bt_UARTsend("Welcome to Stellarilink\r\n");
	//testing tts module
	//initSYN();

	//this has to start after initUART() because it uses bt_UARTsend()
	initTimer0();
	//initTimer1();

	initADC0();

	ROM_SysCtlDelay(1000000);

	//setup pwm
	initPWM(5000);
	pwmSetDuty(500);





	//enable all interrupts
	ROM_IntMasterEnable();
	
	//init pins as low
	//ROM_GPIOPinWrite(GPIO_PORTD_BASE, 0xff, 0);
	
	//initialize Queue
	if(!QueueInit()) bt_UARTsend("Queue Initialized\r\n");
	
	// loop forever
	//step(300, 1, 600000);

	belt_driver(1);

	/*
	 * Main Processing Loop
	 * Task:
	 * Debounce BB1
	 * Debounce BB2
	 * Debounce BB3
	 * ADC Sample - Triggered then Timer based?
	 * Poll/Sample Ferromagnetic
	 * StepperPos Set - Stepper control is timer/interrupt based, StepperPos tells it where to go next
	 * LCD Update?
	 */


	while(1)
	{

		if (firstRun)
		{
			//turn(1);
			//turn(0);
			//turn(1);
			firstRun=0;
		}


		while (_gNextDelay > 14 )
		{
			_gNextDelay--;
			ROM_SysCtlDelay(1000000);
		}


		//belt_driver(drive_go);
		//ROM_SysCtlDelay(5000000);
		//belt_driver(2);
		//ROM_SysCtlDelay(5000000);
		tts_UARTputc('c');
	}
}
void initPeripheralClocks(void)
{
	// enable PORT A,B,D,E,F GPIO and Timer0 peripheral
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA|SYSCTL_PERIPH_GPIOB|SYSCTL_PERIPH_GPIOC|SYSCTL_PERIPH_GPIOD|SYSCTL_PERIPH_GPIOE|SYSCTL_PERIPH_GPIOF);

	ROM_SysCtlDelay(200000);
}


//Initialize the GPIO PINS
void initGPIO(void)
{
	// set LED PORT D pins as outputs
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTD_BASE, (PD_0 | PD_1 | PD_2 | PD_3 | PD_4 | PD_5));
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, (PA_5 | PA_6 | PA_7));
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, (PB_3 | PB_4));
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, redled|bluled|grnled);

	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, (PA_2 | PA_3 | PA_4));
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTD_BASE, (PD_6 | PD_7));

	// set Port A 2,3,4 and Port B 6,7 as inputs
	//ROM_GPIOPinTypeGPIOInput(GPIO_PORTA_BASE, (PA_2 | PA_3 | PA_4));
	//ROM_GPIOPinTypeGPIOInput(GPIO_PORTD_BASE, (PD_6 | PD_7));
	ROM_GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, (btn2));
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, PF_0);

	// set Port B and E stepper pins as output
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, (PB_5 | PB_0 | PB_1));
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, (PE_4 | PE_5 | PE_1));
}

void PortAIntHandler(void)
{
	//moved interrupt clear to end to prevent bouncing since it's too fast
	//ROM_GPIOPinIntClear(GPIO_PORTA_BASE, PA_2);

	ROM_GPIOPinWrite(GPIO_PORTF_BASE, redled, redled);
	
	//re-init readValue
	readValue = 0;
	
	//read from pins
	/*
	if(ROM_GPIOPinRead(GPIO_PORTD_BASE, PD_7)) readValue |= 0x100;
	if(ROM_GPIOPinRead(GPIO_PORTA_BASE, PA_4)) readValue |= 0x010;
	if(ROM_GPIOPinRead(GPIO_PORTA_BASE, PA_3)) readValue |= 0x001;
	*/
	
	ROM_SysCtlDelay(2000000);
	ROM_GPIOPinIntClear(GPIO_PORTA_BASE, PA_2);		//clear interrupt here to prevent softbouce
	ROM_GPIOPinWrite(GPIO_PORTF_BASE, redled, redled);
}

void PortFIntHandler(void)
{
	ROM_GPIOPinIntClear(GPIO_PORTF_BASE, (btn2));
	//ROM_GPIOPinIntClear(GPIO_PORTF_BASE, (btn1|btn2));
	
	//clear leds
	//ROM_GPIOPinWrite(GPIO_PORTA_BASE, 0xff, 0);
	//ROM_GPIOPinWrite(GPIO_PORTD_BASE, 0xff, 0);

	//bt_UARTsend("\033[2J");		//clear screen
	bt_UARTsend("Port F interrupt\r\n");
	
	drive_go ^= 1;
	if(drive_go) belt_driver(2);
	else belt_driver(0);

}

void UART0IntHandler(void)
{
	unsigned long ulStatus;

	ulStatus = ROM_UARTIntStatus(UART0_BASE, true);
	ROM_UARTIntClear(UART0_BASE, ulStatus);
	while(ROM_UARTCharsAvail(UART0_BASE))
	{
		ROM_UARTCharPutNonBlocking(UART0_BASE, ROM_UARTCharGetNonBlocking(UART0_BASE));
	}
}

void UART3IntHandler(void)
{
	unsigned long ulStatus;

	ulStatus = ROM_UARTIntStatus(UART3_BASE, true);
	ROM_UARTIntClear(UART3_BASE, ulStatus);
	while(ROM_UARTCharsAvail(UART3_BASE))
	{
		ROM_UARTCharPutNonBlocking(UART3_BASE, ROM_UARTCharGetNonBlocking(UART3_BASE));
	}
}

void UART4IntHandler(void)
{
	unsigned long ulStatus;

	ulStatus = ROM_UARTIntStatus(UART4_BASE, true);
	ROM_UARTIntClear(UART4_BASE, ulStatus);
	while(ROM_UARTCharsAvail(UART4_BASE))
	{
		ROM_UARTCharPutNonBlocking(UART4_BASE, ROM_UARTCharGetNonBlocking(UART4_BASE));
	}
}

void PortUnlock(void)
{
	//unlock gpio pin PD7 since it's used as a special function
	HWREG(GPIO_PORTD_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY_DD;
	HWREG(GPIO_PORTD_BASE + GPIO_O_CR) |= 0x80;
	HWREG(GPIO_PORTD_BASE + GPIO_O_LOCK) = 0;

	HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY_DD;
	HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= 0x01;
	HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = 0;
}

//-----------Globals for Belt Driver

//define pins here for future reassignment
#define IA PA_3
#define EA PA_4
#define EB PD_7
#define IB PD_6

int belt_dir = 0;		//0 means BREAK!!!

void belt_driver(int direction)
{
	switch(direction)
	{
		case 0: {
				ROM_GPIOPinWrite(GPIO_PORTA_BASE, (IA|EA), 0);
				ROM_GPIOPinWrite(GPIO_PORTD_BASE, (IB|EB), 0);
			} break;
		case 1: {
				ROM_GPIOPinWrite(GPIO_PORTA_BASE, (IA|EA), IA);
				ROM_GPIOPinWrite(GPIO_PORTD_BASE, (IB|EB), 0);
			} break;
		case 2: {
				ROM_GPIOPinWrite(GPIO_PORTA_BASE, (IA|EA), 0);
				ROM_GPIOPinWrite(GPIO_PORTD_BASE, (IB|EB), IB);
			} break;
		default: {
				ROM_GPIOPinWrite(GPIO_PORTA_BASE, (IA|EA), 0);
				ROM_GPIOPinWrite(GPIO_PORTD_BASE, (IB|EB), 0);
			} break;
	}
}

